#pragma once

#include "../core/i_struct.h"
#include "../core/i_blas.h"

namespace idl
{
  template<typename T>
  class CorrespondenceGrad
  {
  public:
    CorrespondenceGrad();
    CorrespondenceGrad(int cx, int cy, int width, const T* const* im_data, int im_width, int im_height);
    CorrespondenceGrad(int cx, int cy, int width, const T* const* im_data);
    CorrespondenceGrad(const CorrespondenceGrad<T>& Corr);
    CorrespondenceGrad& operator=(const CorrespondenceGrad<T>& Corr);
    ~CorrespondenceGrad();
    bool init(int cx, int cy, int width, const T* const* im_data, int im_width, int im_height);
    const Pair<float, float>* data() const {return _data[0];};
    int size() const { return _width*_width; };
    int width() const { return _width; };
    int centroid_x() const { return _cx; };
    int centroid_y() const { return _cy; };
    bool empty() const { return (!_initialized); };
  protected:
    void cleanup();
  protected:
    Pair<float, float> **_data; /*gradient in x, y directions*/
    int _cx, _cy; /*center x, center y*/
    int _width;
    bool _initialized;
  };

  template<typename T>
  CorrespondenceGrad<T>::CorrespondenceGrad() : _data(NULL), _cx(-1), _cy(-1)
  {
    _initialized = false;
  }

  template<typename T>
  CorrespondenceGrad<T>::CorrespondenceGrad(int cx, int cy, int width, const T* const* im_data, int im_width, int im_height) : _cx(cx), _cy(cy), _width(width)
  {
    int i, x, y, r, c, rad0, rad1, first_y, last_y, first_x, last_x;
    if (cx < 0 || cx > im_width - 1 || cy < 0 || cy > im_height - 1)
    {
      return;
    }
    _initialized = false;
    if (im_data)
    {
      _data = i_alloc2<Pair<float, float> >(_width, _width);
      rad0 = _width / 2;
      rad1 = (_width - rad0 - 1);
      first_y = _cy - rad0;
      last_y = _cy + rad1;
      first_x = _cx - rad0;
      last_x = _cx + rad1;
      if (_data)
      {
        /*assign values:*/
        for (r = 0, y = first_y; y <= last_y; ++y, ++r)
        {
          if (y < 1 || y >= im_height - 1)
          {
            for (i = 0; i < width; ++i){ _data[r][i].first = _data[r][i].second = 0.f; }
          }
          else
          {
            for (c = 0, x = first_x; x <= last_x; ++x, ++c)
            {
              if (x < 1 || x >= im_width-1)
              {
                _data[r][c].first = _data[r][c].second = 0.f;
              }
              else
              {
                _data[r][c].first  = (float)im_data[y][x-1] - (float)im_data[y][x+1]; /*dx*/
                _data[r][c].second = (float)im_data[y-1][x] - (float)im_data[y+1][x]; /*dy*/
              }
            }
          }
        }
        _initialized = true;
      }
    }
  }

  /*ingore the image boundary conditions, faster*/
  template<typename T>
  CorrespondenceGrad<T>::CorrespondenceGrad(int cx, int cy, int width, const T* const* im_data) : _cx(cx), _cy(cy), _width(width)
  {
    int x, y, r, c, rad0, rad1, first_y, last_y, first_x, last_x;
    _initialized = false;
    if (im_data)
    {
      _data = i_alloc2<Pair<float, float> >(_width, _width);
      rad0 = _width / 2;
      rad1 = (_width - rad0 - 1);
      first_y = _cy - rad0;
      last_y = _cy + rad1;
      first_x = _cx - rad0;
      last_x = _cx + rad1;
      if (_data)
      {
        /*assign values:*/
        for (r = 0, y = first_y; y <= last_y; ++y, ++r)
        {
          for (c = 0, x = first_x; x <= last_x; ++x, ++c)
          {
            _data[r][c].first = (float)im_data[y][x - 1] - (float)im_data[y][x + 1]; /*dx*/
            _data[r][c].second = (float)im_data[y - 1][x] - (float)im_data[y + 1][x]; /*dy*/
          }
        }
        _initialized = true;
      }
    }
  }

  template<typename T>
  CorrespondenceGrad<T>::CorrespondenceGrad(const CorrespondenceGrad<T>& Corr)
  {
    _cx = Corr.centroid_x();
    _cy = Corr.centroid_y();
    _width = Corr.width();
    _initialized = !Corr.empty();
    if (_initialized && Corr.data())
    {
      _data = i_alloc2<Pair<float, float> >(_width, _width);
      if (!_data)
      {
        _initialized = false;
      }
      else
      {
        for (int i = 0; i < _width*_width; ++i)
        {
          _data[0][i] = Corr.data()[i];
        }
      }
    }
  }

  template<typename T>
  CorrespondenceGrad<T>& CorrespondenceGrad<T>::operator=(const CorrespondenceGrad<T>& Corr)
  {
    if (this->_initialized)
    {
      if (this->_data[0] == Corr.data())
      {
        return(*this);
      }
      else
      {
        this->cleanup();
      }
    }
    this->_cx = Corr.centroid_x();
    this->_cy = Corr.centroid_y();
    this->_width = Corr.width();
    this->_initialized = !Corr.empty();
    if (this->_initialized && Corr.data())
    {

      this->_data = i_alloc2<Pair<float, float> >(_width, _width);
      if (!this->_data)
      {
        this->_initialized = false;
      }
      else
      {
        for (int i = 0; i < _width*_width; ++i)
        {
          (this->_data)[0][i] = Corr.data()[i];
        }
      }
    }
    return(*this);
  }

  template<typename T>
  CorrespondenceGrad<T>::~CorrespondenceGrad()
  {
    i_free2<Pair<float, float> >(_data);
  }

  template<typename T>
  void CorrespondenceGrad<T>::cleanup()
  {
    _initialized = false;
    i_free2<Pair<float, float> >(_data);
    _cx = _cy = 0;
    _width = 0;
  }

  template<typename T>
  bool CorrespondenceGrad<T>::init(int cx, int cy, int width, const T* const* im_data, int im_width, int im_height)
  {
    int x, y, r, c, rad0, rad1, first_y, last_y, first_x, last_x, i;
    if (cx < 0 || cx > im_width - 1 || cy < 0 || cy > im_height - 1)
    {
      return false;
    }
    cleanup();
    _width = width;
    _cx = cx;
    _cy = cy;
    _width = width;
    if (im_data)
    {
      _data = i_alloc2<Pair<float, float> >(_width, _width);
      rad0 = _width / 2;
      rad1 = (_width - rad0 - 1);
      first_y = _cy - rad0;
      last_y = _cy + rad1;
      first_x = _cx - rad0;
      last_x = _cx + rad1;
      if (_data)
      {
        /*assign values:*/
        for (r = 0, y = first_y; y <= last_y; ++y, ++r)
        {
          if (y < 1 || y >= im_height - 1)
          {
            for (i = 0; i < width; ++i){ _data[r][i].first = _data[r][i].second = 0.f; }
          }
          else
          {
            for (c = 0, x = first_x; x <= last_x; ++x, ++c)
            {
              if (x < 1 || x >= im_width - 1)
              {
                _data[r][c].first = _data[r][c].second = 0.f;
              }
              else
              {
                _data[r][c].first = (float)im_data[y][x - 1] - (float)im_data[y][x + 1];  /*dx*/
                _data[r][c].second = (float)im_data[y - 1][x] - (float)im_data[y + 1][x]; /*dy*/
              }
            }
          }
        }
        _initialized = true;
      }
    }
    return _initialized;
  }

  /*This routine returns a floating point sad score [0,1] between two patches, with 0 being the most similar and 1 the least*/
  template <typename T>
  float i_match_grad(const CorrespondenceGrad<T>& patch_x, const CorrespondenceGrad<T>& patch_y, const float max_grad_absdiff = 50.f)
  {
    if (patch_x.empty() || patch_y.empty() || (patch_x.width() != patch_y.width()))
    {
      return 1.f;
    }
    float score = 0.f;
    const Pair<float, float>* data_x = patch_x.data();
    const Pair<float, float>* data_y = patch_y.data();
    int length = patch_x.size();
    for (int i = 0; i<length; ++i)
    {
      score += i_average(i_min(i_abs(data_x[i].first - data_y[i].first), max_grad_absdiff), i_min(i_abs(data_x[i].second - data_y[i].second), max_grad_absdiff));
    }
    return i_div(i_div(score, length), max_grad_absdiff);
  }

} /* namespace idl */